#include <ros/ros.h>
#include <move_base_msgs/MoveBaseAction.h>
#include <actionlib/client/simple_action_client.h>
#include <tcr100/VehicleGoal.h>
#include <tf/tf.h>
#include <stdio.h>
#include <sys/types.h>
#include <sys/ipc.h>
#include <sys/msg.h>
#include <errno.h>
#include <unistd.h>
#include <stdio.h>

#define MSGKEY 10241024

struct msgstru
{
   long msgtype;
   double x;
   double y;
   double rotate;
   double velocity;
};

typedef actionlib::SimpleActionClient<move_base_msgs::MoveBaseAction> MoveBaseClient;

int main(int argc, char** argv)
{
    ros::init(argc, argv, "move_to_goal");
    ros::NodeHandle n;
    ros::Rate r(20.0);
    MoveBaseClient ac("move_base",true);
    while(!ac.waitForServer(ros::Duration(5.0)))
    {
        ROS_INFO("waiting for the move base action server to come up");
    }
    struct msgstru msgs;
    int msgid,ret_value;

    while(ros::ok()){
       msgid = msgget(MSGKEY,IPC_EXCL );/*检查消息队列是否存在 */
       if(msgid < 0){
          printf("msq not existed! errno=%d [%s]\n",errno,strerror(errno));
          sleep(2000);
          continue;
       }
       /*接收消息队列*/
       ret_value = msgrcv(msgid,&msgs,sizeof(struct msgstru),0,0);
       printf("x=[%lf],y=[%lf],rotate=[%lf], pid=[%d]\n",msgs.x,msgs.y,msgs.rotate,getpid());
       move_base_msgs::MoveBaseGoal goal;
       goal.target_pose.header.frame_id = "map";
       goal.target_pose.header.stamp = ros::Time::now();
       goal.target_pose.pose.position.x =msgs.x ;
       goal.target_pose.pose.position.y =msgs.y;
       tf::Quaternion q=tf::createQuaternionFromRPY(0,0,msgs.rotate);
       goal.target_pose.pose.orientation.z =q[2];
       goal.target_pose.pose.orientation.w =q[3];
       ROS_INFO("send goal");
       ac.sendGoal(goal);
    }
}
